// SPDX-License-Identifier: GPL-2.0
/*
 * simmipi driver
 *
 * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
 *
 * V0.0X01.0X01 add poweron function.
 */
 
#include <linux/clk.h>
#include <linux/device.h>
#include <linux/delay.h>
#include <linux/gpio/consumer.h>
#include <linux/i2c.h>
#include <linux/module.h>
#include <linux/pm_runtime.h>
#include <linux/regulator/consumer.h>
#include <linux/sysfs.h>
#include <linux/slab.h>
#include <linux/version.h>
#include <linux/rk-camera-module.h>
#include <media/media-entity.h>
#include <media/v4l2-async.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-subdev.h>
#include <linux/pinctrl/consumer.h>
 
//MEDIA_BUS_FMT_UYVY8_2X8;  //MEDIA_BUS_FMT_YUYV8_2X8
#define SIMMIPI_FMT_YUV (MEDIA_BUS_FMT_UYVY8_2X8)
 
#define DRIVER_VERSION          KERNEL_VERSION(2, 0x01, 0x01)
 
#ifndef V4L2_CID_DIGITAL_GAIN
#define V4L2_CID_DIGITAL_GAIN       V4L2_CID_GAIN
#endif
 
#if 1
#define SIMMIPI_LINK_FREQ_300MHZ 300000000
/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
#define SIMMIPI_PIXEL_RATE       (SIMMIPI_LINK_FREQ_300MHZ*2*2/8)
#define SIMMIPI_XVCLK_FREQ       24000000
 
#else
 
#define SIMMIPI_LINK_FREQ_300MHZ 300000000
/* pixel rate = link frequency * 2 * lanes / BITS_PER_SAMPLE */
#define SIMMIPI_PIXEL_RATE       (SIMMIPI_LINK_FREQ_300MHZ * 2 * 2 / 10)
#define SIMMIPI_XVCLK_FREQ       24000000
 
#endif
 
#define CHIP_ID             0x00d850
#define SIMMIPI_REG_CHIP_ID      0x300a
 
#define SIMMIPI_REG_CTRL_MODE        0x0100
#define SIMMIPI_MODE_SW_STANDBY      0x0
#define SIMMIPI_MODE_STREAMING       BIT(0)
 
#define SIMMIPI_REG_EXPOSURE     0x3500
#define SIMMIPI_EXPOSURE_MIN     4
#define SIMMIPI_EXPOSURE_STEP        1
#define SIMMIPI_VTS_MAX          0x7fff
 
#define SIMMIPI_REG_GAIN_H       0x350a
#define SIMMIPI_REG_GAIN_L       0x350b
#define SIMMIPI_GAIN_H_MASK      0x07
#define SIMMIPI_GAIN_H_SHIFT     8
#define SIMMIPI_GAIN_L_MASK      0xff
#define SIMMIPI_GAIN_MIN     0x10
#define SIMMIPI_GAIN_MAX     0xf8
#define SIMMIPI_GAIN_STEP        1
#define SIMMIPI_GAIN_DEFAULT     0x10
 
#define SIMMIPI_REG_TEST_PATTERN 0x5e00
#define SIMMIPI_TEST_PATTERN_ENABLE  0x80
#define SIMMIPI_TEST_PATTERN_DISABLE 0x0
 
#define SIMMIPI_REG_VTS          0x380e
 
#define REG_NULL            0xFFFF
 
#define SIMMIPI_REG_VALUE_08BIT      1
#define SIMMIPI_REG_VALUE_16BIT      2
#define SIMMIPI_REG_VALUE_24BIT      3
 
#define SIMMIPI_LANES            4
#define SIMMIPI_BITS_PER_SAMPLE      10
 
#define SIMMIPI_CHIP_REVISION_REG    0x302A
#define SIMMIPI_R1A          0xb1
#define SIMMIPI_R2A          0xb2
 
#define OF_CAMERA_PINCTRL_STATE_DEFAULT "rockchip,camera_default"
#define OF_CAMERA_PINCTRL_STATE_SLEEP   "rockchip,camera_sleep"
 
#define SIMMIPI_NAME         "simmipi"
 
static const char * const simmipi_supply_names[] = {
    "avdd",     /* Analog power */
    "dovdd",    /* Digital I/O power */
    "dvdd",     /* Digital core power */
};
 
#define SIMMIPI_NUM_SUPPLIES ARRAY_SIZE(simmipi_supply_names)
 
struct regval {
    u16 addr;
    u8 val;
};
 
struct simmipi_mode {
    u32 width;
    u32 height;
    struct v4l2_fract max_fps;
    u32 hts_def;
    u32 vts_def;
    u32 exp_def;
    const struct regval *reg_list;
};
 
struct simmipi {
    struct i2c_client   *client;
    struct clk      *xvclk;
    struct gpio_desc    *pwr_gpio;
    struct gpio_desc    *reset_gpio;
    struct gpio_desc    *pwdn_gpio;
    struct regulator_bulk_data supplies[SIMMIPI_NUM_SUPPLIES];
 
    struct pinctrl      *pinctrl;
    struct pinctrl_state    *pins_default;
    struct pinctrl_state    *pins_sleep;
 
    struct v4l2_subdev  subdev;
    struct media_pad    pad;
    struct v4l2_ctrl_handler ctrl_handler;
    struct v4l2_ctrl    *exposure;
    struct v4l2_ctrl    *anal_gain;
    struct v4l2_ctrl    *digi_gain;
    struct v4l2_ctrl    *hblank;
    struct v4l2_ctrl    *vblank;
    struct v4l2_ctrl    *test_pattern;
    struct mutex        mutex;
    bool            streaming;
    bool            power_on;
    const struct simmipi_mode *cur_mode;
    u32         module_index;
    const char      *module_facing;
    const char      *module_name;
    const char      *len_name;
};
 
#define to_simmipi(sd) container_of(sd, struct simmipi, subdev)
 
 
static const struct simmipi_mode supported_modes[] = {
    {
        .width = 1920,
        .height = 1080,
        .max_fps = {
            .numerator = 10000,
            .denominator = 300000,
        },
        .exp_def = 0x0600,
        .hts_def = 0x12c0,
        .vts_def = 0x0680,
    },
};
 
static const s64 link_freq_menu_items[] = {
    SIMMIPI_LINK_FREQ_300MHZ
};
 
static const char * const simmipi_test_pattern_menu[] = {
    "Disabled",
    "Vertical Color Bar Type 1",
    "Vertical Color Bar Type 2",
    "Vertical Color Bar Type 3",
    "Vertical Color Bar Type 4"
};
 
 
 
 
static const struct simmipi_mode *
simmipi_find_best_fit(struct v4l2_subdev_format *fmt)
{
    return &supported_modes[0];
}
 
static int simmipi_set_fmt(struct v4l2_subdev *sd,
               struct v4l2_subdev_pad_config *cfg,
              struct v4l2_subdev_format *fmt)
{
    struct simmipi *simmipi = to_simmipi(sd);
    const struct simmipi_mode *mode;
    s64 h_blank, vblank_def;
 
    mutex_lock(&simmipi->mutex);
 
    mode = simmipi_find_best_fit(fmt);
    fmt->format.code = SIMMIPI_FMT_YUV;
    fmt->format.width = mode->width;
    fmt->format.height = mode->height;
    fmt->format.field = V4L2_FIELD_NONE;
    if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
        *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format;
#else
        mutex_unlock(&simmipi->mutex);
        return -ENOTTY;
#endif
    } else {
        simmipi->cur_mode = mode;
        h_blank = mode->hts_def - mode->width;
        __v4l2_ctrl_modify_range(simmipi->hblank, h_blank,
                     h_blank, 1, h_blank);
        vblank_def = mode->vts_def - mode->height;
        __v4l2_ctrl_modify_range(simmipi->vblank, vblank_def,
                     SIMMIPI_VTS_MAX - mode->height,
                     1, vblank_def);
    }
 
    mutex_unlock(&simmipi->mutex);
 
    return 0;
}
 
static int simmipi_get_fmt(struct v4l2_subdev *sd,
               struct v4l2_subdev_pad_config *cfg,
               struct v4l2_subdev_format *fmt)
{
    struct simmipi *simmipi = to_simmipi(sd);
    const struct simmipi_mode *mode = simmipi->cur_mode;
 
    mutex_lock(&simmipi->mutex);
    if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
        fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
#else
        mutex_unlock(&simmipi->mutex);
        return -ENOTTY;
#endif
    } else {
        fmt->format.width = mode->width;
        fmt->format.height = mode->height;
        fmt->format.code = SIMMIPI_FMT_YUV;
        fmt->format.field = V4L2_FIELD_NONE;
    }
    mutex_unlock(&simmipi->mutex);
 
    return 0;
}
 
static int simmipi_enum_mbus_code(struct v4l2_subdev *sd,
                  struct v4l2_subdev_pad_config *cfg,
                  struct v4l2_subdev_mbus_code_enum *code)
{
    if (code->index != 0)
        return -EINVAL;
    code->code = SIMMIPI_FMT_YUV;
 
    return 0;
}
 
static int simmipi_enum_frame_sizes(struct v4l2_subdev *sd,
                    struct v4l2_subdev_pad_config *cfg,
                   struct v4l2_subdev_frame_size_enum *fse)
{
    if (fse->index >= ARRAY_SIZE(supported_modes))
        return -EINVAL;
 
    if (fse->code != SIMMIPI_FMT_YUV)
        return -EINVAL;
 
    fse->min_width  = supported_modes[fse->index].width;
    fse->max_width  = supported_modes[fse->index].width;
    fse->max_height = supported_modes[fse->index].height;
    fse->min_height = supported_modes[fse->index].height;
 
    return 0;
}
 
static int simmipi_enable_test_pattern(struct simmipi *simmipi, u32 pattern)
{
    return 0;
}
 
static int simmipi_g_frame_interval(struct v4l2_subdev *sd,
                    struct v4l2_subdev_frame_interval *fi)
{
    struct simmipi *simmipi = to_simmipi(sd);
    const struct simmipi_mode *mode = simmipi->cur_mode;
 
    mutex_lock(&simmipi->mutex);
    fi->interval = mode->max_fps;
    mutex_unlock(&simmipi->mutex);
 
    return 0;
}
 
static void simmipi_get_module_inf(struct simmipi *simmipi,
                   struct rkmodule_inf *inf)
{
    memset(inf, 0, sizeof(*inf));
    strlcpy(inf->base.sensor, SIMMIPI_NAME, sizeof(inf->base.sensor));
    strlcpy(inf->base.module, simmipi->module_name,
        sizeof(inf->base.module));
    strlcpy(inf->base.lens, simmipi->len_name, sizeof(inf->base.lens));
}
 
static long simmipi_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
{
    struct simmipi *simmipi = to_simmipi(sd);
    long ret = 0;
 
    switch (cmd) {
    case RKMODULE_GET_MODULE_INFO:
        simmipi_get_module_inf(simmipi, (struct rkmodule_inf *)arg);
        break;
    case RKMODULE_SET_QUICK_STREAM:
        break;
    default:
        ret = -ENOIOCTLCMD;
        break;
    }
 
    return ret;
}
 
#ifdef CONFIG_COMPAT
static long simmipi_compat_ioctl32(struct v4l2_subdev *sd,
                   unsigned int cmd, unsigned long arg)
{
    void __user *up = compat_ptr(arg);
    struct rkmodule_inf *inf;
    struct rkmodule_awb_cfg *cfg;
    long ret;
 
    switch (cmd) {
    case RKMODULE_GET_MODULE_INFO:
        inf = kzalloc(sizeof(*inf), GFP_KERNEL);
        if (!inf) {
            ret = -ENOMEM;
            return ret;
        }
 
        ret = simmipi_ioctl(sd, cmd, inf);
        if (!ret)
            ret = copy_to_user(up, inf, sizeof(*inf));
        kfree(inf);
        break;
    case RKMODULE_AWB_CFG:
        cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
        if (!cfg) {
            ret = -ENOMEM;
            return ret;
        }
 
        ret = copy_from_user(cfg, up, sizeof(*cfg));
        if (!ret)
            ret = simmipi_ioctl(sd, cmd, cfg);
        kfree(cfg);
    case RKMODULE_SET_QUICK_STREAM:
        break;
        break;
    default:
        ret = -ENOIOCTLCMD;
        break;
    }
 
    return ret;
}
#endif
 
static int __simmipi_start_stream(struct simmipi *simmipi)
{
    int ret;
 
 
    /* In case these controls are set before streaming */
    mutex_unlock(&simmipi->mutex);
    ret = v4l2_ctrl_handler_setup(&simmipi->ctrl_handler);
    mutex_lock(&simmipi->mutex);
 
 
    return 0;
}
 
static int __simmipi_stop_stream(struct simmipi *simmipi)
{
    return 0;
}
 
 
 
static int simmipi_s_stream(struct v4l2_subdev *sd, int on)
{
    struct simmipi *simmipi = to_simmipi(sd);
    struct i2c_client *client = simmipi->client;
    int ret = 0;
 
    mutex_lock(&simmipi->mutex);
    on = !!on;
    if (on == simmipi->streaming)
        goto unlock_and_return;
 
    if (on) {
        ret = pm_runtime_get_sync(&client->dev);
        if (ret < 0) {
            pm_runtime_put_noidle(&client->dev);
            goto unlock_and_return;
        }
 
        ret = __simmipi_start_stream(simmipi);
        if (ret) {
            v4l2_err(sd, "start stream failed while write regs\n");
            pm_runtime_put(&client->dev);
            goto unlock_and_return;
        }
    } else {
        __simmipi_stop_stream(simmipi);
        pm_runtime_put(&client->dev);
    }
 
    simmipi->streaming = on;
 
unlock_and_return:
    mutex_unlock(&simmipi->mutex);
 
    return ret;
}
 
static int simmipi_s_power(struct v4l2_subdev *sd, int on)
{
    struct simmipi *simmipi = to_simmipi(sd);
    struct i2c_client *client = simmipi->client;
    int ret = 0;
 
    mutex_lock(&simmipi->mutex);
 
    /* If the power state is not modified - no work to do. */
    if (simmipi->power_on == !!on)
        goto unlock_and_return;
 
    if (on) {
        ret = pm_runtime_get_sync(&client->dev);
        if (ret < 0) {
            pm_runtime_put_noidle(&client->dev);
            goto unlock_and_return;
        }
#if 0
        ret = simmipi_write_array(simmipi->client, simmipi_global_regs);
        if (ret) {
            v4l2_err(sd, "could not set init registers\n");
            pm_runtime_put_noidle(&client->dev);
            goto unlock_and_return;
        }
#endif
        simmipi->power_on = true;
    } else {
        pm_runtime_put(&client->dev);
        simmipi->power_on = false;
    }
 
unlock_and_return:
    mutex_unlock(&simmipi->mutex);
 
    return ret;
}
 
/* Calculate the delay in us by clock rate and clock cycles */
static inline u32 simmipi_cal_delay(u32 cycles)
{
    return DIV_ROUND_UP(cycles, SIMMIPI_XVCLK_FREQ / 1000 / 1000);
}
 
static int __simmipi_power_on(struct simmipi *simmipi)
{
    int ret;
    u32 delay_us;
    struct device *dev = &simmipi->client->dev;
 
    if (!IS_ERR_OR_NULL(simmipi->pins_default)) {
        ret = pinctrl_select_state(simmipi->pinctrl,
                       simmipi->pins_default);
        if (ret < 0)
            dev_err(dev, "could not set pins\n");
    }
#if 0
    ret = clk_set_rate(simmipi->xvclk, SIMMIPI_XVCLK_FREQ);
    if (ret < 0) {
        dev_err(dev, "Failed to set xvclk rate (24MHz)\n");
        return ret;
    }
    if (clk_get_rate(simmipi->xvclk) != SIMMIPI_XVCLK_FREQ)
        dev_warn(dev, "xvclk mismatched, modes are based on 24MHz\n");
 
    ret = clk_prepare_enable(simmipi->xvclk);
    if (ret < 0) {
        dev_err(dev, "Failed to enable xvclk\n");
        return ret;
    }
 
    //if (!IS_ERR(simmipi->pwr_gpio))
    //  gpiod_set_value_cansleep(simmipi->pwr_gpio, 1);
    //usleep_range(500, 1000);
#endif
    if (!IS_ERR(simmipi->reset_gpio))
        gpiod_set_value_cansleep(simmipi->reset_gpio, 0);
 
    ret = regulator_bulk_enable(SIMMIPI_NUM_SUPPLIES, simmipi->supplies);
    if (ret < 0) {
        dev_err(dev, "Failed to enable regulators\n");
        goto disable_clk;
    }
 
    if (!IS_ERR(simmipi->reset_gpio))
        gpiod_set_value_cansleep(simmipi->reset_gpio, 1);
 
    usleep_range(500, 1000);
    if (!IS_ERR(simmipi->pwdn_gpio))
        gpiod_set_value_cansleep(simmipi->pwdn_gpio, 1);
 
    /* 8192 cycles prior to first SCCB transaction */
    delay_us = simmipi_cal_delay(8192);
    usleep_range(delay_us, delay_us * 2);
 
    return 0;
 
disable_clk:
    clk_disable_unprepare(simmipi->xvclk);
 
    return ret;
}
 
static void __simmipi_power_off(struct simmipi *simmipi)
{
    int ret;
    struct device *dev = &simmipi->client->dev;
 
    if (!IS_ERR(simmipi->pwdn_gpio))
        gpiod_set_value_cansleep(simmipi->pwdn_gpio, 0);
    clk_disable_unprepare(simmipi->xvclk);
    if (!IS_ERR(simmipi->reset_gpio))
        gpiod_set_value_cansleep(simmipi->reset_gpio, 0);
    if (!IS_ERR_OR_NULL(simmipi->pins_sleep)) {
        ret = pinctrl_select_state(simmipi->pinctrl,
                       simmipi->pins_sleep);
        if (ret < 0)
            dev_dbg(dev, "could not set pins\n");
    }
    if (!IS_ERR(simmipi->pwr_gpio))
        gpiod_set_value_cansleep(simmipi->pwr_gpio, 0);
    regulator_bulk_disable(SIMMIPI_NUM_SUPPLIES, simmipi->supplies);
}
 
static int simmipi_runtime_resume(struct device *dev)
{
    struct i2c_client *client = to_i2c_client(dev);
    struct v4l2_subdev *sd = i2c_get_clientdata(client);
    struct simmipi *simmipi = to_simmipi(sd);
 
    return __simmipi_power_on(simmipi);
}
 
static int simmipi_runtime_suspend(struct device *dev)
{
    struct i2c_client *client = to_i2c_client(dev);
    struct v4l2_subdev *sd = i2c_get_clientdata(client);
    struct simmipi *simmipi = to_simmipi(sd);
 
    __simmipi_power_off(simmipi);
 
    return 0;
}
 
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
static int simmipi_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
{
    struct simmipi *simmipi = to_simmipi(sd);
    struct v4l2_mbus_framefmt *try_fmt =
                v4l2_subdev_get_try_format(sd, fh->pad, 0);
    const struct simmipi_mode *def_mode = &supported_modes[0];
 
    mutex_lock(&simmipi->mutex);
    /* Initialize try_fmt */
    try_fmt->width = def_mode->width;
    try_fmt->height = def_mode->height;
    try_fmt->code = SIMMIPI_FMT_YUV;
    try_fmt->field = V4L2_FIELD_NONE;
 
    mutex_unlock(&simmipi->mutex);
    /* No crop or compose */
 
    return 0;
}
#endif
 
 
static int simmipi_enum_frame_interval(struct v4l2_subdev *sd,
                       struct v4l2_subdev_pad_config *cfg,
                       struct v4l2_subdev_frame_interval_enum *fie)
{
    if (fie->index >= ARRAY_SIZE(supported_modes))
        return -EINVAL;
 
    if (fie->code != MEDIA_BUS_FMT_SBGGR10_1X10)
        return -EINVAL;
 
    fie->width = supported_modes[fie->index].width;
    fie->height = supported_modes[fie->index].height;
    fie->interval = supported_modes[fie->index].max_fps;
    return 0;
}
static int simmipi_g_mbus_config(struct v4l2_subdev *sd, unsigned int pad_id,
                 struct v4l2_mbus_config *config)
{
 
    u32 val = 0;
    val = V4L2_MBUS_CSI2_2_LANE | V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
    config->type = V4L2_MBUS_CSI2_DPHY;
    config->flags = val; 
         
    return 0;
}
static const struct dev_pm_ops simmipi_pm_ops = {
    SET_RUNTIME_PM_OPS(simmipi_runtime_suspend,
               simmipi_runtime_resume, NULL)
};
 
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
static const struct v4l2_subdev_internal_ops simmipi_internal_ops = {
    .open = simmipi_open,
};
#endif
 
static const struct v4l2_subdev_core_ops simmipi_core_ops = {
    .s_power = simmipi_s_power,
    .ioctl = simmipi_ioctl,
#ifdef CONFIG_COMPAT
    .compat_ioctl32 = simmipi_compat_ioctl32,
#endif
};
 
static const struct v4l2_subdev_video_ops simmipi_video_ops = {
    .s_stream = simmipi_s_stream,
    .g_frame_interval = simmipi_g_frame_interval,
};
 
static const struct v4l2_subdev_pad_ops simmipi_pad_ops = {
    .enum_mbus_code = simmipi_enum_mbus_code,
    .enum_frame_size = simmipi_enum_frame_sizes,
    .enum_frame_interval = simmipi_enum_frame_interval,
    .get_fmt = simmipi_get_fmt,
    .set_fmt = simmipi_set_fmt,
    .get_mbus_config = simmipi_g_mbus_config,
};
 
static const struct v4l2_subdev_ops simmipi_subdev_ops = {
    .core   = &simmipi_core_ops,
    .video  = &simmipi_video_ops,
    .pad    = &simmipi_pad_ops,
};
 
static int simmipi_set_ctrl(struct v4l2_ctrl *ctrl)
{
    struct simmipi *simmipi = container_of(ctrl->handler,
                         struct simmipi, ctrl_handler);
 
    struct i2c_client *client = simmipi->client;
    s64 max;
    int ret = 0;
 
    /* Propagate change of current control to all related controls */
    switch (ctrl->id) {
    case V4L2_CID_VBLANK:
        /* Update max exposure while meeting expected vblanking */
        max = simmipi->cur_mode->height + ctrl->val - 4;
        __v4l2_ctrl_modify_range(simmipi->exposure,
                     simmipi->exposure->minimum, max,
                     simmipi->exposure->step,
                     simmipi->exposure->default_value);
        break;
    }
 
    if (pm_runtime_get(&client->dev) <= 0)
        return 0;
 
    switch (ctrl->id) {
    case V4L2_CID_EXPOSURE:
        /* 4 least significant bits of expsoure are fractional part */
        ret = 0;
        break;
    case V4L2_CID_ANALOGUE_GAIN:
        ret = 0;
        break;
    case V4L2_CID_VBLANK:
        ret = 0;
        break;
    case V4L2_CID_TEST_PATTERN:
        ret = simmipi_enable_test_pattern(simmipi, ctrl->val);
        break;
    default:
        dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
             __func__, ctrl->id, ctrl->val);
        break;
    }
 
    pm_runtime_put(&client->dev);
 
    return ret;
 
}
 
static const struct v4l2_ctrl_ops simmipi_ctrl_ops = {
    .s_ctrl = simmipi_set_ctrl,
};
 
static int simmipi_initialize_controls(struct simmipi *simmipi)
{
    const struct simmipi_mode *mode;
    struct v4l2_ctrl_handler *handler;
    struct v4l2_ctrl *ctrl;
    s64 exposure_max, vblank_def;
    u32 h_blank;
    int ret;
 
    handler = &simmipi->ctrl_handler;
    mode = simmipi->cur_mode;
    ret = v4l2_ctrl_handler_init(handler, 8);
    if (ret)
        return ret;
    handler->lock = &simmipi->mutex;
 
    ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ,
                      0, 0, link_freq_menu_items);
    if (ctrl)
        ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
    v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE,
              0, SIMMIPI_PIXEL_RATE, 1, SIMMIPI_PIXEL_RATE);
 
    h_blank = mode->hts_def - mode->width;
    simmipi->hblank = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK,
                h_blank, h_blank, 1, h_blank);
    if (simmipi->hblank)
        simmipi->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
    vblank_def = mode->vts_def - mode->height;
    simmipi->vblank = v4l2_ctrl_new_std(handler, &simmipi_ctrl_ops,
                V4L2_CID_VBLANK, vblank_def,
                SIMMIPI_VTS_MAX - mode->height,
                1, vblank_def);
 
    exposure_max = mode->vts_def - 4;
    simmipi->exposure = v4l2_ctrl_new_std(handler, &simmipi_ctrl_ops,
                V4L2_CID_EXPOSURE, SIMMIPI_EXPOSURE_MIN,
                exposure_max, SIMMIPI_EXPOSURE_STEP,
                mode->exp_def);
 
    simmipi->anal_gain = v4l2_ctrl_new_std(handler, &simmipi_ctrl_ops,
                V4L2_CID_ANALOGUE_GAIN, SIMMIPI_GAIN_MIN,
                SIMMIPI_GAIN_MAX, SIMMIPI_GAIN_STEP,
                SIMMIPI_GAIN_DEFAULT);
 
    simmipi->test_pattern = v4l2_ctrl_new_std_menu_items(handler,
                &simmipi_ctrl_ops, V4L2_CID_TEST_PATTERN,
                ARRAY_SIZE(simmipi_test_pattern_menu) - 1,
                0, 0, simmipi_test_pattern_menu);
 
    if (handler->error) {
        ret = handler->error;
        dev_err(&simmipi->client->dev,
            "Failed to init controls(%d)\n", ret);
        goto err_free_handler;
    }
 
    simmipi->subdev.ctrl_handler = handler;
 
    return 0;
 
err_free_handler:
    v4l2_ctrl_handler_free(handler);
 
    return ret;
}
 
static int simmipi_check_sensor_id(struct simmipi *simmipi,
                   struct i2c_client *client)
{
    struct device *dev = &simmipi->client->dev;
    u32 id = 0;
 
    dev_info(dev, "Detected OV%06x sensor, REVISION 0x%x\n", CHIP_ID, id);
 
    return 0;
}
 
static int simmipi_configure_regulators(struct simmipi *simmipi)
{
    unsigned int i;
 
    for (i = 0; i < SIMMIPI_NUM_SUPPLIES; i++)
        simmipi->supplies[i].supply = simmipi_supply_names[i];
 
    return devm_regulator_bulk_get(&simmipi->client->dev,
                       SIMMIPI_NUM_SUPPLIES,
                       simmipi->supplies);
}
 
static int simmipi_probe(struct i2c_client *client,
             const struct i2c_device_id *id)
{
    struct device *dev = &client->dev;
    struct device_node *node = dev->of_node;
    struct simmipi *simmipi;
    struct v4l2_subdev *sd;
    char facing[2];
    int ret;
 
    dev_info(dev, "driver version: %02x.%02x.%02x",
        DRIVER_VERSION >> 16,
        (DRIVER_VERSION & 0xff00) >> 8,
        DRIVER_VERSION & 0x00ff);
 
    simmipi = devm_kzalloc(dev, sizeof(*simmipi), GFP_KERNEL);
    if (!simmipi)
        return -ENOMEM;
 
    ret = of_property_read_u32(node, RKMODULE_CAMERA_MODULE_INDEX,
                   &simmipi->module_index);
    ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_FACING,
                       &simmipi->module_facing);
    ret |= of_property_read_string(node, RKMODULE_CAMERA_MODULE_NAME,
                       &simmipi->module_name);
    ret |= of_property_read_string(node, RKMODULE_CAMERA_LENS_NAME,
                       &simmipi->len_name);
    if (ret) {
        dev_err(dev, "could not get module information!\n");
        return -EINVAL;
    }
 
    simmipi->client = client;
    simmipi->cur_mode = &supported_modes[0];
 
    // simmipi->xvclk = devm_clk_get(dev, "xvclk");
    // if (IS_ERR(simmipi->xvclk)) {
        // dev_err(dev, "Failed to get xvclk\n");
        // return -EINVAL;
    // }
     
 
    simmipi->pwr_gpio = devm_gpiod_get(dev, "power", GPIOD_OUT_LOW);
    if (IS_ERR(simmipi->pwr_gpio))
        dev_warn(dev, "Failed to get pwr-gpios\n");
 
    simmipi->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
    if (IS_ERR(simmipi->reset_gpio))
        dev_warn(dev, "Failed to get reset-gpios\n");
 
    simmipi->pwdn_gpio = devm_gpiod_get(dev, "pwdn", GPIOD_OUT_LOW);
    if (IS_ERR(simmipi->pwdn_gpio))
        dev_warn(dev, "Failed to get pwdn-gpios\n");
 
    ret = simmipi_configure_regulators(simmipi);
    if (ret) {
        dev_err(dev, "Failed to get power regulators\n");
        return ret;
    }
 
    simmipi->pinctrl = devm_pinctrl_get(dev);
    if (!IS_ERR(simmipi->pinctrl)) {
        simmipi->pins_default =
            pinctrl_lookup_state(simmipi->pinctrl,
                         OF_CAMERA_PINCTRL_STATE_DEFAULT);
        if (IS_ERR(simmipi->pins_default))
            dev_err(dev, "could not get default pinstate\n");
 
        simmipi->pins_sleep =
            pinctrl_lookup_state(simmipi->pinctrl,
                         OF_CAMERA_PINCTRL_STATE_SLEEP);
        if (IS_ERR(simmipi->pins_sleep))
            dev_err(dev, "could not get sleep pinstate\n");
    }
 
    mutex_init(&simmipi->mutex);
 
    sd = &simmipi->subdev;
    v4l2_i2c_subdev_init(sd, client, &simmipi_subdev_ops);
    ret = simmipi_initialize_controls(simmipi);
    if (ret)
        goto err_destroy_mutex;
 
    ret = __simmipi_power_on(simmipi);
    if (ret)
        goto err_free_handler;
 
    ret = simmipi_check_sensor_id(simmipi, client);
    if (ret)
        goto err_power_off;
 
#ifdef CONFIG_VIDEO_V4L2_SUBDEV_API
    sd->internal_ops = &simmipi_internal_ops;
    sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
#endif
#if defined(CONFIG_MEDIA_CONTROLLER)
    simmipi->pad.flags = MEDIA_PAD_FL_SOURCE;
    sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
    ret = media_entity_pads_init(&sd->entity, 1, &simmipi->pad);
    if (ret < 0)
        goto err_power_off;
#endif
 
    memset(facing, 0, sizeof(facing));
    if (strcmp(simmipi->module_facing, "back") == 0)
        facing[0] = 'b';
    else
        facing[0] = 'f';
 
    snprintf(sd->name, sizeof(sd->name), "m%02d_%s_%s %s",
         simmipi->module_index, facing,
         SIMMIPI_NAME, dev_name(sd->dev));
    ret = v4l2_async_register_subdev_sensor_common(sd);
    if (ret) {
        dev_err(dev, "v4l2 async register subdev failed\n");
        goto err_clean_entity;
    }
 
    pm_runtime_set_active(dev);
    pm_runtime_enable(dev);
    pm_runtime_idle(dev);
 
    printk("sdi simmipi probe end\n");
 
    return 0;
 
err_clean_entity:
#if defined(CONFIG_MEDIA_CONTROLLER)
    media_entity_cleanup(&sd->entity);
#endif
err_power_off:
    __simmipi_power_off(simmipi);
err_free_handler:
    v4l2_ctrl_handler_free(&simmipi->ctrl_handler);
err_destroy_mutex:
    mutex_destroy(&simmipi->mutex);
 
    return ret;
}
 
static int simmipi_remove(struct i2c_client *client)
{
    struct v4l2_subdev *sd = i2c_get_clientdata(client);
    struct simmipi *simmipi = to_simmipi(sd);
 
    v4l2_async_unregister_subdev(sd);
#if defined(CONFIG_MEDIA_CONTROLLER)
    media_entity_cleanup(&sd->entity);
#endif
    v4l2_ctrl_handler_free(&simmipi->ctrl_handler);
    mutex_destroy(&simmipi->mutex);
 
    pm_runtime_disable(&client->dev);
    if (!pm_runtime_status_suspended(&client->dev))
        __simmipi_power_off(simmipi);
    pm_runtime_set_suspended(&client->dev);
 
    return 0;
}
 
#if IS_ENABLED(CONFIG_OF)
static const struct of_device_id simmipi_of_match[] = {
    { .compatible = "lide,simmipi" },
    {},
};
MODULE_DEVICE_TABLE(of, simmipi_of_match);
#endif
 
static const struct i2c_device_id simmipi_match_id[] = {
    { "lide,simmipi", 0 },
    { },
};
 
static struct i2c_driver simmipi_i2c_driver = {
    .driver = {
        .name = SIMMIPI_NAME,
        .pm = &simmipi_pm_ops,
        .of_match_table = of_match_ptr(simmipi_of_match),
    },
    .probe      = &simmipi_probe,
    .remove     = &simmipi_remove,
    .id_table   = simmipi_match_id,
};
 
static int __init sensor_mod_init(void)
{
    return i2c_add_driver(&simmipi_i2c_driver);
}
 
static void __exit sensor_mod_exit(void)
{
    i2c_del_driver(&simmipi_i2c_driver);
}
 
device_initcall_sync(sensor_mod_init);
module_exit(sensor_mod_exit);
 
MODULE_DESCRIPTION("OmniVision simmipi sensor driver");
MODULE_LICENSE("GPL v2");